# -*- coding:utf-8 -*-

robotIP = "192.168.1.14"
PORT = 9559

from naoqi import ALProxy
import time
import math


config = [["MaxStepX", 0.08],
          ["MaxStepY", 0.16],
          ["MaxStepTheta", 0.524],
          ["MaxStepFrequency", 0.9],
          ["StepHeight", 0.03]]

Config = [["MaxStepX", 0.06],
           ["MaxStepY", 0.13],
           ["MaxStepTheta", 0.4],
           ["MaxStepFrequency", 0.7],
           ["StepHeight", 0.03],
           ["TorsoWx", 0.0],
           ["TorsoWy", 0.0]]

motion = ALProxy("ALMotion", robotIP, PORT)
posture = ALProxy("ALRobotPosture", robotIP, PORT)
# tts = ALProxy("ALTextToSpeech", robotIP, PORT)

motion.wakeUp()
posture.goToPosture("StandInit", 0.5)

try:

    motion.wakeUp()

    motion.move(5, 0, 0, Config)  # 起步
    time.sleep(3)
    print(0)

    motion.move(5, 0, 0, config)
    time.sleep(4)
    print(1)

    motion.move(5, 0, -0.2, config)  # 一弯
    time.sleep(5.5)
    print(2)

    motion.move(5, 0, 0, config)
    time.sleep(4.5)
    print(3)

    motion.move(5, 0, 0.03, config)
    time.sleep(6)
    print(4)

    motion.move(5, 0, 0.12, config)  # 二弯
    time.sleep(6)
    print(5)

    motion.move(5, 0, 0.15, config)
    time.sleep(6)
    print(6)

    motion.move(5, 0, 0, Config)
    time.sleep(10)
    print(7)

    motion.stopMove()
    motion.rest()

except Exception, errorMsg:
    print str(errorMsg)
    print "This example is not allowed on this robot."
    exit()